#include <iostream>
#include <opencv2/core.hpp>
#include "image_io.h"
#include "body.h"
#include "config.h"
#include "gl_render.h"
#include "region_tracker.h"
#include "viewer.h"
#include "dataset.h"
#include "region_model.h"
#include "timer.h"

namespace fs = std::filesystem;

int test_gl(Body* body, Camera* camera, Render* render, std::vector<Pose>& gt_poses) {
  Timer timer;
  int idx = 0;
  while (camera->UpdateImage()) {
    body->set_body2world_pose(gt_poses[idx++]);

    //timer.Start();
    //root_viewer->UpdateViewer();
    //std::cout << timer.ElapsedMicros() << std::endl;

    if (idx > 1000) break;
#if 1
    timer.Start();
    render->RenderSilhouette();
    render->FetchSilhouetteImage();
    std::cout << timer.ElapsedMicros() << std::endl;
#endif
    auto keycode = cv::waitKey(1);
    if (27 == keycode) { break; }
  }
  return 0;
}

int test_track() {
  auto config = ManualConfigBuilder()
    .fx(12.54f).fy(12.54f)
    .cx(24.4f).cy(233.4f)
    .image_width(640).image_height(512)
    .data_path("")
    .Build();

  std::cout << config.cx << std::endl;

#ifdef WIN32
  std::string mesh_path = "d:/dataset/RBOT_dataset/ape/ape.obj";
  std::string frame_dir = "d:/dataset/RBOT_dataset/ape/frames";
  std::string pose_path = "d:/dataset/RBOT_dataset/poses_first.txt";
  fs::path camera_video_path("d:/tmp/cam0.mp4");
  fs::path viewer_video_path("d:/tmp/viewer0.mp4");
#else
  std::string mesh_path = "/home/hh/dataset/rbot/ape/ape.obj";
  std::string frame_dir = "/home/hh/dataset/rbot/ape/frames";
  std::string pose_path = "/home/hh/dataset/rbot/poses_first.txt";
  fs::path camera_video_path("/home/hh/cam0.mp4");
  fs::path viewer_video_path("/home/hh/viewer0.mp4");
#endif

  auto init_pose = Pose::Identity();
  std::unique_ptr<Body> body(BodyBuilder()
    .mesh_path(mesh_path, 0.001f)
    .body2world_pose(init_pose)
    .Build());
  std::cout << body->mesh()->indices.size() << std::endl;

  auto tracker = RegionTrackerBuilder()
    .learning_rate_b(0.2f)
    .learning_rate_f(0.3f)
    .standard_deviations({1,2,3})
    .Build();

  int image_width = 640;
  int image_height = 512;
  Intrinsics intrinsics = { 650.048f, 647.183f, 324.328f, 257.323f, image_width, image_height };
  
  //auto camera_writer = VideoImageWriter::Build(camera_video_path, image_width, image_height);
  auto camera_writer = nullptr;
  std::unique_ptr<Camera> camera(Camera::Build(
    intrinsics,
    FolderImageLoader::Build(frame_dir, "png"),
    camera_writer));

  std::vector<Pose> gt_poses;
  ReadPosesRBOTDataset(pose_path, &gt_poses);

  std::unique_ptr<Render> render(GLRender::Build(camera->intrinsics()));
  render->AddBody(body.get());
  
  //auto video_writer = VideoImageWriter::Build(viewer_video_path, image_width, image_height);
  auto viewer_writer = nullptr;
  std::unique_ptr<RootViewer> root_viewer(
    RootViewer::Build("normal viewer", viewer_writer));
  auto image_viewer = ImageViewer::Build(camera.get());
  root_viewer->AddViewer(image_viewer);
  image_viewer->AddViewer(NormalViewer::Build(render.get()));
  image_viewer->AddViewer(AxisViewer::Build(camera.get(), body.get()));

#if 0
  RegionModelBuilder()
    .sphere_radius(0.3f)
    .n_contour_points(200)
    .sphere_radius(0.8f)
    .Build(body.get(), "d:/tmp");
#endif
  return test_gl(body.get(), camera.get(), render.get(), gt_poses);
}

int main() {
  return test_track();
}